#include "OgreRefAppFieldPlayerOwnedStates.h"
#include "OgreRefAppFieldPlayer.h"
#include "OgreRefAppBall.h"
#include "OgreRefAppGoal.h"
#include "OgreRefAppConstants.h"
#include "OgreRefAppSoccerPitch.h"

#define __DEBUGON__


namespace OgreRefApp
{

//************************************************************************ Global state

    GlobalPlayerState* GlobalPlayerState::Instance()
    {
        static GlobalPlayerState instance;

        return &instance;
    }


    void GlobalPlayerState::Execute(FieldPlayer* player)
    {

        //if a player is in possession and close to the ball reduce his max speed
        if ((player->BallWithinReceivingRange()) && (player->isControllingPlayer()))
        {
            player->setMaxLinearVelocity(PlayerMaxSpeedWithBall);
        }

        else
        {
            player->setMaxLinearVelocity(PlayerMaxSpeedWithoutBall);
        }

    }


    bool GlobalPlayerState::OnMessage(FieldPlayer* player, OgreRefApp::ApplicationObject::CollisionInfo& telegram)
    {

        switch (telegram.Msg)
        {
        case Msg_ReceiveBall:
        {
            //set the target
            player->Steering()->SetTarget(telegram.position);

            //change state
            player->GetFSM()->ChangeState(ReceiveBall::Instance());

            return true;
        }

        break;

        case Msg_SupportAttacker:
        {
            //if already supporting just return
            if (player->GetFSM()->isInState(SupportAttacker::Instance()))
            {
                return true;
            }

            //set the target to be the best supporting position
            player->Steering()->SetTarget(player->Team()->GetSupportSpot());

            //change the state
            player->GetFSM()->ChangeState(SupportAttacker::Instance());

            return true;
        }

        break;

        case Msg_Wait:
        {
            //change the state
            player->GetFSM()->ChangeState(Wait::Instance());

            return true;
        }

        break;

        case Msg_GoHome:
        {
                    std::cout << "GlobalPlayerState::OnMessage" << telegram.Msg << std::endl;

            player->SetDefaultHomeRegion();

            player->GetFSM()->ChangeState(ReturnToHomeRegion::Instance());

            return true;
        }

        break;

        case Msg_PassToMe:
        {

            //get the position of the player requesting the pass
            ApplicationObject* receiver = telegram.sender;


            //if the ball is not within kicking range or their is already a
            //receiving player, this player cannot pass the ball to the player
            //making the request.
            if (player->Team()->Receiver() != NULL ||
                    !player->BallWithinKickingRange() )
            {


                return true;
            }

            //make the pass

            player->getBall()->Kick(receiver->getPosition() - player->getBall()->getPosition(),
                                    MaxPassingForce);


            ApplicationObject::CollisionInfo collInfo;
            collInfo.Msg = Msg_ReceiveBall;
            collInfo.sender = player;


            receiver->handleMessage( collInfo );


            //change state
            player->GetFSM()->ChangeState(Wait::Instance());

            player->FindSupport();

            return true;
        }

        break;

        }//end switch

        return false;
    }




//***************************************************************************** CHASEBALL

    ChaseBall* ChaseBall::Instance()
    {
        static ChaseBall instance;

        return &instance;
    }


    void ChaseBall::Enter(FieldPlayer* player)
    {
        player->Steering()->SeekOn( player->Pitch()->ball()->getPosition() );
        player->Steering()->ArriveOff();
        player->Steering()->PursuitOff();

    }

    void ChaseBall::Execute(FieldPlayer* player)
    {
        //if the ball is within kicking range the player changes state to KickBall.
        if (player->BallWithinKickingRange())
        {
            player->GetFSM()->ChangeState(KickBall::Instance());

            return;
        }

        //if the player is the closest player to the ball then he should keep
        //chasing it
        if (player->isClosestTeamMemberToBall())
        {
            player->Steering()->SetTarget(player->getBall()->getPosition());

            return;
        }

        //if the player is not closest to the ball anymore, he should return back
        //to his home region and wait for another opportunity
        player->GetFSM()->ChangeState(ReturnToHomeRegion::Instance());
    }


    void ChaseBall::Exit(FieldPlayer* player)
    {
        player->Steering()->SeekOff();
    }



//*****************************************************************************SUPPORT ATTACKING PLAYER

    SupportAttacker* SupportAttacker::Instance()
    {
        static SupportAttacker instance;

        return &instance;
    }


    void SupportAttacker::Enter(FieldPlayer* player)
    {
        player->Steering()->ArriveOn();

        player->Steering()->SetTarget(player->Team()->GetSupportSpot());


    }

    void SupportAttacker::Execute(FieldPlayer* player)
    {
        //if his team loses control go back home
        if (!player->Team()->InControl())
        {
            player->GetFSM()->ChangeState(ReturnToHomeRegion::Instance());
            return;
        }


        //if the best supporting spot changes, change the steering target
        if (player->Team()->GetSupportSpot() != player->Steering()->GetTarget())
        {
            player->Steering()->SetTarget(player->Team()->GetSupportSpot());

            player->Steering()->ArriveOn();
        }

        //if this player has a shot at the goal AND the attacker can pass
        //the ball to him the attacker should pass the ball to this player

        if ( player->Team()->CanShoot(Ogre::Vector3( player->getPosition() ),
                                      MaxShootingForce))
        {
            player->Team()->RequestPass(player);
        }


        //if this player is located at the support spot and his team still have
        //possession, he should remain still and turn to face the ball
        if (player->AtTarget())
        {
            player->Steering()->ArriveOff();

            //the player should keep his eyes on the ball!
            player->TrackBall();

            player->setLinearVelocity(Ogre::Vector3(0,0,0));

            //if not threatened by another player request a pass
            if (!player->isThreatened())
            {
                player->Team()->RequestPass(player);
            }
        }
    }


    void SupportAttacker::Exit(FieldPlayer* player)
    {
        //set supporting player to null so that the team knows it has to
        //determine a new one.
        player->Team()->SetSupportingPlayer(NULL);

        player->Steering()->ArriveOff();
    }




//************************************************************************ RETURN TO HOME REGION

    ReturnToHomeRegion* ReturnToHomeRegion::Instance()
    {
        static ReturnToHomeRegion instance;

        return &instance;
    }


    void ReturnToHomeRegion::Enter(FieldPlayer* player)
    {
        player->Steering()->ArriveOn();


        if (!player->HomeRegion()->Inside(Ogre::Vector2( player->Steering()->GetTarget().x, player->Steering()->GetTarget().y), Region::halfsize))
        {
            player->Steering()->SetTarget(Ogre::Vector3( player->HomeRegion()->Center().x, player->HomeRegion()->Center().y, PlayerHeight / 2 ));
        }


    }

    void ReturnToHomeRegion::Execute(FieldPlayer* player)
    {
        if (player->Pitch()->gameOn())
        {
            //if the ball is nearer this player than any other team member  &&
            //there is not an assigned receiver && the goalkeeper does not gave
            //the ball, go chase it
            if ( player->isClosestTeamMemberToBall() &&
                    (player->Team()->Receiver() == NULL) &&
                    !player->Pitch()->goalKeeperHasBall())
            {
                player->GetFSM()->ChangeState(ChaseBall::Instance());

                return;
            }
        }

        //if game is on and close enough to home, change state to wait and set the
        //player target to his current position.(so that if he gets jostled out of
        //position he can move back to it)
        if (player->Pitch()->gameOn() && player->HomeRegion()->Inside(Ogre::Vector2( player->getPosition().x, player->getPosition().y),
                Region::halfsize))
        {
            player->Steering()->SetTarget(player->getPosition());
            player->GetFSM()->ChangeState(Wait::Instance());
        }
        //if game is not on the player must return much closer to the center of his
        //home region
        else if (!player->Pitch()->gameOn() && player->AtTarget())
        {
            player->GetFSM()->ChangeState(Wait::Instance());
        }
    }

    void ReturnToHomeRegion::Exit(FieldPlayer* player)
    {
        player->Steering()->ArriveOff();
    }




//***************************************************************************** WAIT

    Wait* Wait::Instance()
    {
        static Wait instance;

        return &instance;
    }


    void Wait::Enter(FieldPlayer* player)
    {

        //if the game is not on make sure the target is the center of the player's
        //home region. This is ensure all the players are in the correct positions
        //ready for kick off
        if (!player->Pitch()->gameOn())
        {
            player->Steering()->SetTarget(Ogre::Vector3( player->HomeRegion()->Center().x, player->HomeRegion()->Center().y, PlayerHeight/2 ));
        }
    }

    void Wait::Execute(FieldPlayer* player)
    {
        player->Steering()->SeparationOff();
        player->Steering()->SeekOff();
        /*
        #ifdef __DEBUGON__

    std::cout << std::endl << player->HomeRegion()->Center().x << std::endl;
    std::cout << std::endl << player->getPosition() << std::endl;
    #endif
    */
        player->Steering()->SetTarget(Ogre::Vector3( player->HomeRegion()->Center().x, player->HomeRegion()->Center().y, PlayerHeight/2 ));
        //if the player has been jostled out of position, get back in position
        if (!player->AtTarget())
        {
            player->Steering()->ArriveOn();


            return;
        }

        else
        {

            player->Steering()->ArriveOff();

            player->setLinearVelocity(Ogre::Vector3(0,0,0));

            //the player should keep his eyes on the ball!
            player->TrackBall();
        }

        //if this player's team is controlling AND this player is not the attacker
        //AND is further up the field than the attacker he should request a pass.
        if ( player->Team()->InControl()    &&
                (!player->isControllingPlayer()) &&
                player->isAheadOfAttacker() )
        {
            player->Team()->RequestPass(player);

            return;
        }

        if (player->Pitch()->gameOn())
        {
            //if the ball is nearer this player than any other team member  AND
            //there is not an assigned receiver AND neither goalkeeper has
            //the ball, go chase it
            if (player->isClosestTeamMemberToBall() &&
                    player->Team()->Receiver() == NULL  &&
                    !player->Pitch()->goalKeeperHasBall())
            {
                player->GetFSM()->ChangeState(ChaseBall::Instance());

                return;
            }
        }

    }

    void Wait::Exit(FieldPlayer* player){}




//************************************************************************ KICK BALL

    KickBall* KickBall::Instance()
    {
        static KickBall instance;

        return &instance;
    }


    void KickBall::Enter(FieldPlayer* player)
    {
        //let the team know this player is controlling
        player->Team()->SetControllingPlayer(player);

        //the player can only make so many kick attempts per second.
        if (!player->isReadyForNextKick())
        {
            player->GetFSM()->ChangeState(ChaseBall::Instance());
        }

    }

    void KickBall::Execute(FieldPlayer* player)
    {
        player->Team()->SetControllingPlayer(player);

        //calculate the dot product of the vector pointing to the ball
        //and the player's heading
        Ogre::Vector3 ToBall = player->getBall()->getPosition() - player->getPosition();
        //Real   dot    = player->Heading.dotProduct(Vec2DNormalize(ToBall));

        //cannot kick the ball if the goalkeeper is in possession or if it is
        //behind the player or if there is already an assigned receiver. So just
        //continue chasing the ball
        /*if (player->Team()->Receiver() != NULL   ||
            player->Pitch()->GoalKeeperHasBall() ||
            (dot < 0) ) */

        if (player->Team()->Receiver() != NULL   ||
                player->Pitch()->goalKeeperHasBall() )
        {


            player->GetFSM()->ChangeState(ChaseBall::Instance());

            return;
        }


        /* Attempt a shot at the goal */

        //if a shot is possible, this vector will hold the position along the
        //opponent's goal line the player should aim for.
        Ogre::Vector3*    BallTarget = new Ogre::Vector3();

        //the dot product is used to adjust the shooting force. The more
        //directly the ball is ahead, the more forceful the kick
        //double power = Prm.MaxShootingForce * dot;
        Real power = MaxShootingForce;

        //if it is determined that the player could score a goal from this position
        //OR if he should just kick the ball anyway, the player will attempt
        //to make the shot

        if (player->Team()->CanShoot(Ogre::Vector3( player->getBall()->getPosition() ),
                                     power,
                                     BallTarget)                   ||
                (RandFloat() < ChancePlayerAttemptsPotShot) )
        {

        #ifdef __DEBUGON__
        std::cout << "shootou "<< player->Team()->Opponents()->HomeGoal()->Center() << std::endl;
        #endif
            //add some noise to the kick. We don't want players who are
            //too accurate! The amount of noise can be adjusted by altering
            //Prm.PlayerKickingAccuracy
            BallTarget = new Ogre::Vector3(player->Team()->OpponentsGoal()->Center());
            BallTarget = new Ogre::Vector3( player->getBall()->AddNoiseToKick(Ogre::Vector3( player->getBall()->getPosition()  ), BallTarget) );

            //this is the direction the ball will be kicked in
            Ogre::Vector3 KickDirection = Ogre::Vector3(BallTarget->x, BallTarget->y, BallTarget->z);

            player->getBall()->Kick(KickDirection, 620);
        //player->getBall()->Kick(player->Team()->Opponents()->HomeGoal()->Center(), 620);

            //change state
            player->GetFSM()->ChangeState(Wait::Instance());

            player->FindSupport();

            return;
        }


        /* Attempt a pass to a player */

        //if a receiver is found this will point to it
        PlayerBase* receiver = NULL;

        //power = Prm.MaxPassingForce * dot;
        power = MaxPassingForce;

        //test if there are any potential candidates available to receive a pass
        if (player->isThreatened()  &&
                player->Team()->FindPass(player,
                                         receiver,
                                         Ogre::Vector3( BallTarget->x, BallTarget->y, BallTarget->z ),
                                         power,
                                         MinPassDistance))
        {


            //add some noise to the kick

            //BallTarget = new Ogre::Vector3( player->getBall()->AddNoiseToKick(player->getBall()->getPosition(), player->Team()->Receiver()->getPosition()  ));

            //Ogre::Vector3 KickDirection = Ogre::Vector3(BallTarget->x, BallTarget->y, BallTarget->z) - player->getBall()->getPosition();

            //player->getBall()->Kick(KickDirection, 120);

            player->getBall()->Kick(player->Team()->Receiver()->getPosition() , 420);





            ApplicationObject::CollisionInfo collInfo;
            collInfo.Msg = Msg_ReceiveBall;
            collInfo.sender = player;
            collInfo.position = player->getBall()->getPosition();


            player->Team()->Receiver()->handleMessage( collInfo );

            //the player should wait at his current position unless instruced
            //otherwise
            player->GetFSM()->ChangeState(Wait::Instance());

            player->FindSupport();

            return;
        }

        //cannot shoot or pass, so dribble the ball upfield
        else
        {

            player->FindSupport();



            player->GetFSM()->ChangeState(Dribble::Instance());
        }


    }


//*************************************************************************** DRIBBLE

    Dribble* Dribble::Instance()
    {
        static Dribble instance;

        return &instance;
    }


    void Dribble::Enter(FieldPlayer* player)
    {
        //let the team know this player is controlling
        player->Team()->SetControllingPlayer(player);


    }

    void Dribble::Execute(FieldPlayer* player)
    {


        //double dot = player->Team()->HomeGoal()->Facing().Dot(player->Heading());

        //if the ball is between the player and the home goal, it needs to swivel
        // the ball around by doing multiple small kicks and turns until the player
        //is facing in the correct direction
        /*
        if (dot < 0)
        {
          //the player's heading is going to be rotated by a small amount (Pi/4)
          //and then the ball will be kicked in that direction
          Ogre::Vector3 direction = player->Heading();

          //calculate the sign (+/-) of the angle between the player heading and the
          //facing direction of the goal so that the player rotates around in the
          //correct direction
          double angle = QuarterPi * -1 *
                       player->Team()->HomeGoal()->Facing().Sign(player->Heading());

          Vec2DRotateAroundOrigin(direction, angle);

          //this value works well whjen the player is attempting to control the
          //ball and turn at the same time
          const double KickingForce = 0.8;

          player->Ball()->Kick(direction, KickingForce);
        }
        */

        //kick the ball down the field
        //else
        //{
/*
        Ogre::Vector3* BallTarget = new Ogre::Vector3( player->getBall()->AddNoiseToKick(player->getBall()->getPosition(), BallTarget ));

        Ogre::Vector3 KickDirection = Ogre::Vector3(BallTarget->x, BallTarget->y, BallTarget->z) - player->getBall()->getPosition();
*/
        player->getBall()->Kick(player->Team()->Opponents()->HomeGoal()->Center(), 30);

          //player->getBall()->Kick(Ogre::Vector3(400,666,6666), 1200);
          //player->getBall()->setLinearVelocity(Ogre::Vector3(0,0,6666));


        //}

        //the player has kicked the ball so he must now change state to follow it
        player->GetFSM()->ChangeState(ChaseBall::Instance());

        return;
    }



//************************************************************************     RECEIVEBALL

    ReceiveBall* ReceiveBall::Instance()
    {
        static ReceiveBall instance;

        return &instance;
    }


    void ReceiveBall::Enter(FieldPlayer* player)
    {
        //let the team know this player is receiving the ball
        player->Team()->SetReceiver(player);

        //this player is also now the controlling player
        player->Team()->SetControllingPlayer(player);

        //there are two types of receive behavior. One uses arrive to direct
        //the receiver to the position sent by the passer in its telegram. The
        //other uses the pursuit behavior to pursue the ball.
        //This statement selects between them dependent on the probability
        //ChanceOfUsingArriveTypeReceiveBehavior, whether or not an opposing
        //player is close to the receiving player, and whether or not the receiving
        //player is in the opponents 'hot region' (the third of the pitch closest
        //to the opponent's goal
        const Real PassThreatRadius = 10.0;

        if (( player->InHotRegion() ||
                RandFloat() < ChanceOfUsingArriveTypeReceiveBehavior) &&
                !player->Team()->isOpponentWithinRadius(player->getPosition(), PassThreatRadius))
        {
            player->Steering()->ArriveOn();

        }
        else
        {
            player->Steering()->PursuitOn();


        }
    }

    void ReceiveBall::Execute(FieldPlayer* player)
    {
        #ifdef __DEBUGON__
        std::cout << "ReceiveBall::Execute "<< player->Team()->Opponents()->HomeGoal()->Center() << std::endl;
        #endif
        //if the ball comes close enough to the player or if his team lose control
        //he should change state to chase the ball
        if (player->BallWithinReceivingRange() || !player->Team()->InControl())
        {
            #ifdef __DEBUGON__
            std::cout << "player->BallWithinReceivingRange() "<< player->Team()->Opponents()->HomeGoal()->Center() << std::endl;
            #endif
            player->GetFSM()->ChangeState(ChaseBall::Instance());

            return;
        }

        if (player->Steering()->isPursuitOn())
        {
            player->Steering()->SetTargetAgent1(player->getBall());
        }

        if (player->Steering()->isArriveOn())
        {
            player->Steering()->SetTarget(player->getBall()->getPosition());
        }

        //if the player has 'arrived' at the steering target he should wait and
        //turn to face the ball
        if (player->AtTarget())
        {
            player->Steering()->ArriveOff();
            player->Steering()->PursuitOff();
            player->TrackBall();
            player->setLinearVelocity(Ogre::Vector3(0,0,0));
        }
    }

    void ReceiveBall::Exit(FieldPlayer* player)
    {
        player->Steering()->ArriveOff();
        player->Steering()->PursuitOff();

        player->Team()->SetReceiver(NULL);
    }
}
